#include "DJ_Driver.h"
#include "PID.h"
#include "BSP.h"

static DJ_Motor_t *Motors[8]; // 电机参数结构体指针数组, ID即为索引

/* CAN中断回调函数 */
void DJ_CAN_Callback(CAN_RxHeaderTypeDef *pHeader, uint8_t *pBuf)
{
    for (uint8_t i = 0; i < 8; i++)
    {
        if (pHeader->StdId == Motors[i]->ID)
        {
            Motors[i]->last_angle = Motors[i]->angle;
            Motors[i]->angle = (uint16_t)(pBuf[0] << 8 | pBuf[1]);
            Motors[i]->speed = (uint16_t)(pBuf[2] << 8 | pBuf[3]);
            Motors[i]->current = (uint16_t)(pBuf[4] << 8 | pBuf[5]);

            if (Motors[i]->angle - Motors[i]->last_angle > 4096)
            {
                Motors[i]->round_count--;
            }
            else if (Motors[i]->angle - Motors[i]->last_angle < -4096)
            {
                Motors[i]->round_count++;
            }
            Motors[i]->total_angle = (Motors[i]->round_count + (Motors[i]->angle - Motors[i]->offset_angle) / 8192.0f) * 360.0f / Motors[i]->decratio;

            return;
        }
    }
}

/**
 * 电机参数初始化
 * 不可以重复初始化
 */
void DJ_Init(DJ_Motor_t *motor, uint8_t Motor_ID, uint8_t Motor_Type)
{
    if (Motor_Type == M2006)
    {
        motor->decratio = DJ_M2006_DECRATIO;
        // 速度PID初始化
        PID_Init(&motor->PID_Speed, 5.0f, 0.5f, 0.0f, 16000.0f, 1000.0f);

        // 角度PID初始化
        PID_Init(&motor->PID_Angle, 150.0f, 0.5f, 10.0f, 25000.0f, 100.0f);
        PID_Init(&motor->PID_SpeedOfAngle, 5.0f, 0.0f, 0.0f, 16000.0f, 500.0f);
    }
    else if (Motor_Type == M3508)
    {
        motor->decratio = DJ_M3508_DECRATIO;
        // 速度PID初始化
        PID_Init(&motor->PID_Speed, 5.0f, 0.5f, 0.0f, 16000.0f, 1000.0f);

        // 角度PID初始化
        PID_Init(&motor->PID_Angle, 100.0f, 0.3f, 5.0f, 5000.0f, 30.0f);
        PID_Init(&motor->PID_SpeedOfAngle, 5.0f, 0.0f, 0.0f, 16000.0f, 200.0f);
    }

    motor->ID = DJ_L_ID + Motor_ID;
    motor->motor_type = Motor_Type;
    Motors[Motor_ID - 1] = motor;
}

//--------------------------------------------------------------------------
// 大疆电机控制
osThreadId_t DjTaskHandle;
const osThreadAttr_t DjTask_attributes = {
    .name = "DjTask",
    .stack_size = 128 * 4,
    .priority = (osPriority_t)osPriorityNormal,
};

void StartDJTask(void *argument)
{
    uint8_t data[8];

    while (1)
    {
        if (osThreadFlagsGet() == 0x0001) // 如果标志为0x0001, 开启角度控制
        {
            /* 控制电机角度 */
            for (uint8_t i = 0; i < 8; i++)
            {
                Motors[i]->offset_angle = Motors[i]->last_angle; // 获取偏移角度, 提高精准度
                // 串级PID控制
                PID_Calc(&Motors[i]->PID_Angle, (float)(Motors[i]->total_angle), (float)Motors[i]->setAngle);
                PID_Calc(&Motors[i]->PID_SpeedOfAngle, (float)(Motors[i]->speed), Motors[i]->PID_Angle.out);
                Motors[i]->setCurrent = (int16_t)Motors[i]->PID_SpeedOfAngle.out;
            }
        }
        else if (osThreadFlagsGet() == 0x0002) // 如果标志为0x0002, 开启速度控制
        {
            /* 控制电机速度 */
            for (uint8_t i = 0; i < 8; i++)
            {
                PID_Calc(&Motors[i]->PID_Speed, (float)(Motors[i]->speed), (float)Motors[i]->setSpeed);
                Motors[i]->setCurrent = (int16_t)Motors[i]->PID_Speed.out;
            }
        }

        /* 控制电机电流 */
        data[0] = Motors[0]->setCurrent >> 8;
        data[1] = Motors[0]->setCurrent;
        data[2] = Motors[1]->setCurrent >> 8;
        data[3] = Motors[1]->setCurrent;
        data[4] = Motors[2]->setCurrent >> 8;
        data[5] = Motors[2]->setCurrent;
        data[6] = Motors[3]->setCurrent >> 8;
        data[7] = Motors[3]->setCurrent;
        CAN_Transmit(&DJ_MOTOR_CAN, DJ_L_ID, data);
        data[0] = Motors[4]->setCurrent >> 8;
        data[1] = Motors[4]->setCurrent;
        data[2] = Motors[5]->setCurrent >> 8;
        data[3] = Motors[5]->setCurrent;
        data[4] = Motors[6]->setCurrent >> 8;
        data[5] = Motors[6]->setCurrent;
        data[6] = Motors[7]->setCurrent >> 8;
        data[7] = Motors[7]->setCurrent;
        CAN_Transmit(&DJ_MOTOR_CAN, DJ_H_ID, data);

        osDelay(2);
    }
}

/* 大疆电机运行任务初始化 */
inline void DJ_TaskInit(void)
{
    AttachInterrupt_CAN(&DJ_MOTOR_CAN, DJ_CAN_Callback);
    DjTaskHandle = osThreadNew(StartDJTask, NULL, &DjTask_attributes);
}

/* 设置电机角度 */
inline void DJ_SetAngle(DJ_Motor_t *motor, int16_t angle)
{
    motor->setAngle = angle;
    osThreadFlagsSet(DjTaskHandle, 0x0001);
}

/* 设置电机速度 */
inline void DJ_SetSpeed(DJ_Motor_t *motor, int16_t speed)
{
    motor->setSpeed = speed;
    osThreadFlagsSet(DjTaskHandle, 0x0002);
}

/* 电机角度和圈数清零 */
inline void DJ_ClearAngle(DJ_Motor_t *motor)
{
    motor->total_angle = 0;
    motor->round_count = 0;
}
